#include "slepbase.h"
#include "epph.h"


void mcLeastC(slep_matrix* A, slep_matrix* Y, double z, OPTS* opts,
			  slep_matrix* X, slep_vector* funVal, slep_vector* valueL){

	double q, lambda ;
	int k;
	slep_matrix *ATY, *AX;

	k = Y->n;
	assert( Y->m == A->m);
	assert( z>0.0);
	sll_opts(opts);
	if( hasGot(opts, SlepOPT_q))
		(q=2.0 , opts->q = 2.0);
	else{
		q = opts->q;
		assert( SLEPDoubleApproxEquals(q,2.0));
	}

	initNormalization(A,opts);

	ATY = slepAllocDenseMatrix(X->m, X->n);
	AX = slepAllocDenseMatrix(Y->m, Y->n);

	// ATy
	slep_dgemm(SlepTranspose,SlepNoTranspose,1.0,A,Y,0.0,ATY);

	if( opts->init == 2)
		slep_zerom(X);
	else{
		if( hasGot(opts,SlepOPT_X0)){
			slep_dmcopy(opts->X0,X);
			assert( opts->X0->m == A->n && opts->X0->n==k);
		}else
			slep_dmcopy(ATY,X);
	}
	// TODO Changed it to NORMALIZEDMM
	slep_dgemm(SlepNoTranspose,SlepNoTranspose,1.0,A,X,0.0,AX);

	if(opts->init == 0){
		// TODO ratio multiply
	}

	/* Armijo Goldstein line search scheme */
	if( opts->mFlag == 0 && opts->lFlag==0){
		int bFlag = 0, iterStep;
		double L = 1.0;
		double alphap = 0.0 , alpha = 1.0, beta;
		double l_sum , r_sum;
		double lambda0;
		int steps;
		slep_matrix *Xp, *XXp, *AXp, *S;
		slep_matrix *AS, *ATAS, *G, *V, *AV, *AXY;

		Xp = slepCopyToNewMatrix(X);
		AXp = slepCopyToNewMatrix(AX);
		XXp = slepAllocDenseMatrix(Xp->m, Xp->n);
		S = slepAllocDenseMatrix(Xp->m, Xp->n);
		AS = slepAllocDenseMatrix(AXp->m, AXp->n);
		ATAS = slepAllocDenseMatrix(Xp->m, Xp->n);
		G = slepAllocDenseMatrix(Xp->m, Xp->n);
		V = slepAllocDenseMatrix(Xp->m, Xp->n);
		AV = slepAllocDenseMatrix(AXp->m, AXp->n);
		AXY = slepAllocDenseMatrix(AXp->m, AXp->n);
		slep_zerom(XXp);

		lambda0 = 0.0;

		for( iterStep = 0; iterStep < opts->maxIter; iterStep ++ ){
			beta = (alphap - 1.0) / alpha;
			slep_dmcopy(X,S);
			slep_daXpbY(SlepNoTranspose, beta, XXp,1.0,S);
			
			slep_dmcopy(AX,AS);
			slep_daXpbY(SlepNoTranspose, -beta, AXp, 1.0+beta, AS);

			slep_dgemm(SlepTranspose,SlepNoTranspose, 1.0,A,AS,0.0,ATAS);

			slep_dmcopy(ATAS, G);
			slep_daXpbY(SlepNoTranspose, -1.0, ATY, 1.0, G);

			slep_dmcopy(X,Xp);
			slep_dmcopy(AX,AXp);
			for(;;){
				slep_dmcopy(S,V);
				slep_daXpbY(SlepNoTranspose, -1.0/L, G, 1.0, V);

				ep21d(X->data.mtrx,&lambda,&steps,V->data.mtrx,X->m, X->n, z, lambda0);
				lambda0 = lambda;

				slep_dmcopy(X, V);
				slep_daXpbY(SlepNoTranspose, -1.0, S, 1.0, V);

				slep_dgemm(SlepNoTranspose, SlepNoTranspose, 1.0, A, X, 0.0 , AX);
				slep_dmcopy(AX, AV);
				slep_daXpbY(SlepNoTranspose, -1.0, AS, 1.0, AV);

				r_sum = slep_dfrbnrm(V); r_sum = r_sum * r_sum;
				l_sum = slep_dfrbnrm(AV); l_sum = l_sum * l_sum;

				if( r_sum <= 1e-20){
					bFlag = 1;
					break;
				}
				if( l_sum <= r_sum * L)
					break;
				else
					L = SLEPMAX(2*L , l_sum/r_sum);
			}
			valueL->data.vctr[iterStep] = L;
			alphap = alpha;
			alpha = ( 1.0 + sqrt(4.0 * alpha * alpha + 1.0)) / 2.0;
			slep_dmcopy(X,XXp);
			slep_daXpbY(SlepNoTranspose, -1.0, Xp, 1.0, XXp);
			slep_dmcopy(AX, AXY);
			slep_daXpbY(SlepNoTranspose, -1.0, Y, 1.0 , AXY);
			funVal->data.vctr[iterStep] = slep_dfrbnrm(AXY);
			funVal->data.vctr[iterStep] = funVal->data.vctr[iterStep] * funVal->data.vctr[iterStep] / 2.0;

			if(bFlag)
				break;
			if( opts->tFlag == 0){
				if( iterStep >= 2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < opts->tol)
						break;
				}
			}else if( opts->tFlag == 1){
				if(iterStep>=2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < (opts->tol)*(funVal->data.vctr[iterStep-1]) )
						break;
				}
			}else if(opts->tFlag == 2){
				if( funVal->data.vctr[iterStep] <= opts->tol)
					break;
			}else if(opts->tFlag == 3){
				if(slep_dfrbnrm(XXp) <= opts->tol)
					break;
			}else if(opts->tFlag == 4){
				if( slep_dfrbnrm(XXp) <= (opts->tol) * max(slep_dfrbnrm(Xp),1))
					break;
			}else if(opts->tFlag == 5){
				if( iterStep >= opts->maxIter)
					break;
			}
		}
		slepFreeMatrix(Xp);
		slepFreeMatrix(XXp);
		slepFreeMatrix(AXp);
		slepFreeMatrix(S);

		slepFreeMatrix(AS);
		slepFreeMatrix(ATAS);
		slepFreeMatrix(G);
		slepFreeMatrix(V);
		slepFreeMatrix(AV);
		slepFreeMatrix(AXY);
		funVal->dim = iterStep;
		valueL->dim = iterStep;

	}

	/* Adaptive line search */
	if( opts->lFlag==1 ){
		int bFlag = 0, iterStep;
		double L = 1.0;
		double l_sum , r_sum;
		double gamma, alpha, alphap, beta;
		slep_matrix *Xp, *XXp, *AXp, *S;
		slep_matrix *AS, *ATAS, *G, *V, *AV, *AXY;

		slep_matrix *ATAX, *ATAXp;
		slep_matrix *Xnew, *AXnew;
		double tao, lambda0;
		int steps;

		gamma = 1.0;
		lambda0 = 0.0;

		Xp = slepCopyToNewMatrix(X);
		AXp = slepCopyToNewMatrix(AX);
		XXp = slepAllocDenseMatrix(Xp->m, Xp->n);
		S = slepAllocDenseMatrix(Xp->m, Xp->n);
		AS = slepAllocDenseMatrix(AXp->m, AXp->n);
		ATAS = slepAllocDenseMatrix(Xp->m, Xp->n);
		G = slepAllocDenseMatrix(Xp->m, Xp->n);
		V = slepAllocDenseMatrix(Xp->m, Xp->n);
		AV = slepAllocDenseMatrix(AXp->m, AXp->n);
		AXY = slepAllocDenseMatrix(AXp->m, AXp->n);

		ATAX = slepAllocDenseMatrix(Xp->m, Xp->n);
		ATAXp = slepAllocDenseMatrix(Xp->m, Xp->n);
		Xnew = slepAllocDenseMatrix(Xp->m, Xp->n); 
		AXnew = slepAllocDenseMatrix(AXp->m, AXp->n);

		slep_zerom(XXp);
		//ATAX
		slep_dgemm(SlepTranspose, SlepNoTranspose, 1.0, A, AX, 0.0, ATAX);

		for( iterStep = 0; iterStep < opts->maxIter; iterStep ++ ){
			slep_dmcopy(ATAX, ATAXp);

			if(iterStep != 0)
				slep_dgemm(SlepTranspose, SlepNoTranspose, 1.0, A, AX, 0.0, ATAX);

			for(;;){
				if(iterStep != 0){
					alpha = ( -gamma + sqrt(gamma*gamma + 4.0*L*gamma))/(2.0*L);
					beta = (gamma - gamma * alphap) / (alphap * gamma + alphap * L * alpha);

					slep_dmcopy(X,S);
					slep_daXpbY(SlepNoTranspose, beta, XXp, 1.0, S);

					slep_dmcopy(AX, AS);
					slep_daXpbY(SlepNoTranspose,-beta, AXp,1.0+beta, AS);

					slep_dmcopy(ATAX, ATAS);
					slep_daXpbY(SlepNoTranspose,-beta, ATAXp,1.0+beta, ATAS);
				}else{
					alpha = (-1.0 + sqrt(5.0))/2.0;
					beta = 0.0;
					slep_dmcopy(X,S);
					slep_dmcopy(AX,AS);
					slep_dmcopy(ATAX,ATAS);
				}
				slep_dmcopy(ATAS, G);
				slep_daXpbY(SlepNoTranspose, -1.0, ATY, 1.0, G);

				slep_dmcopy(S,V);
				slep_daXpbY(SlepNoTranspose, -1.0/L, G, 1.0, V);

				ep21d(Xnew->data.mtrx,&lambda,&steps,V->data.mtrx,X->m,X->n,z,lambda0);
				lambda0 = lambda;

				slep_dmcopy(Xnew, V);
				slep_daXpbY(SlepNoTranspose, -1.0, S, 1.0, V);

				slep_dgemm(SlepNoTranspose, SlepNoTranspose, 1.0, A, Xnew, 0.0 , AXnew);

				slep_dmcopy(AXnew, AV);
				slep_daXpbY(SlepNoTranspose, -1.0, AS, 1.0, AV);

				r_sum = slep_dfrbnrm(V); r_sum = r_sum * r_sum;
				l_sum = slep_dfrbnrm(AV); l_sum = l_sum * l_sum;

				if( r_sum <= 1e-20){
					bFlag = 1;
					break;
				}
				if( l_sum <= r_sum * L)
					break;
				else
					L = SLEPMAX(2*L , l_sum/r_sum);
			}

			gamma = L * alpha * alpha;
			alphap = alpha;

			valueL->data.vctr[iterStep] = L;
			tao = L * r_sum / l_sum ;
			if(tao >= 5.0)
				L = L * 0.8;

			slep_dmcopy(X, Xp);
			slep_dmcopy(Xnew, X);
			slep_dmcopy(X,XXp);
			slep_daXpbY(SlepNoTranspose, -1.0, Xp, 1.0, XXp);
			slep_dmcopy(AX, AXp);
			slep_dmcopy(AXnew, AX);

			slep_dmcopy(AX, AXY);
			slep_daXpbY(SlepNoTranspose, -1.0, Y, 1.0 , AXY);

			funVal->data.vctr[iterStep] = slep_dfrbnrm(AXY);
			funVal->data.vctr[iterStep] = funVal->data.vctr[iterStep] * funVal->data.vctr[iterStep] / 2.0;

			if(bFlag)
				break;
			if( opts->tFlag == 0){
				if( iterStep >= 2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < opts->tol)
						break;
				}
			}else if( opts->tFlag == 1){
				if(iterStep>=2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < (opts->tol)*(funVal->data.vctr[iterStep-1]) )
						break;
				}
			}else if(opts->tFlag == 2){
				if( funVal->data.vctr[iterStep] <= opts->tol)
					break;
			}else if(opts->tFlag == 3){
				if(slep_dfrbnrm(XXp) <= opts->tol)
					break;
			}else if(opts->tFlag == 4){
				if( slep_dfrbnrm(XXp) <= (opts->tol) * max(slep_dfrbnrm(Xp),1))
					break;
			}else if(opts->tFlag == 5){
				if( iterStep >= opts->maxIter)
					break;
			}
		}
		slepFreeMatrix(Xp);
		slepFreeMatrix(XXp);
		slepFreeMatrix(AXp);
		slepFreeMatrix(S);

		slepFreeMatrix(AS);
		slepFreeMatrix(ATAS);
		slepFreeMatrix(G);
		slepFreeMatrix(V);
		slepFreeMatrix(AV);
		slepFreeMatrix(AXY);

		slepFreeMatrix(ATAX);
		slepFreeMatrix(ATAXp);
		slepFreeMatrix(Xnew);
		slepFreeMatrix(AXnew);

		funVal->dim = iterStep;
		valueL->dim = iterStep;

	}
	slepFreeMatrix(ATY);
	slepFreeMatrix(AX);
}